/*
 * Copyright 2022 MindMotion Microelectronics Co., Ltd.
 * All rights reserved.
 *
 * SPDX-License-Identifier: BSD-3-Clause
 */

#include "CANopen.h"
#include "board_init.h"

#define APP_TIM_UPDATE_STEP    1000000
#define APP_TIM_UPDATE_PERIOD  1000

#define TMR_TASK_INTERVAL      (1000)          /* Interval of tmrTask thread in microseconds */
#define INCREMENT_1MS(var)     (var++)         /* Increment 1ms variable in tmrTask */

/**
 * User-defined CAN base structure, passed as argument to CO_init.
 */
struct CANbase {
    uintptr_t baseAddress;  /**< Base address of the CAN module */
};

/* Global variables and objects */
volatile uint32_t   CO_timer1ms = 0U;   /* variable increments each millisecond */
volatile bool app_tx_flag = false; /* send SDO req flag. */

void app_tim_init(void);

/* main ***********************************************************************/
int main(void){
    CO_NMT_reset_cmd_t reset = CO_RESET_NOT;

    /* Configure microcontroller. */
    BOARD_Init();
    printf("\r\nco_sdo_server example.\r\n");

    /* initialize EEPROM */
    /* Not used. */

    /* increase variable each startup. Variable is stored in EEPROM.
     * TODO: store variables in EEPROM. Now everything is stored in RAM
     */
    OD_powerOnCounter++;

    while(reset != CO_RESET_APP){
/* CANopen communication reset - initialize CANopen objects *******************/
        CO_ReturnError_t err;
        uint16_t timer1msPrevious;

        /* disable CAN and CAN interrupts */
        struct CANbase canBase = {
            .baseAddress = 0u,  /* CAN module address */
        };

        /* initialize CANopen */
        err = CO_init(&canBase, OD_CANNodeID, OD_CANBitRate);
        if(err != CO_ERROR_NO){
            while(1);
            /* CO_errorReport(CO->em, CO_EM_MEMORY_ALLOCATION_ERROR, CO_EMC_SOFTWARE_INTERNAL, err); */
        }

        /* Configure Timer interrupt function for execution every 1 millisecond */
        app_tim_init();

        /* start CAN */
        CO_CANsetNormalMode(CO->CANmodule[0]);

        reset = CO_RESET_NOT;
        timer1msPrevious = CO_timer1ms;

        /* Configure Timer interrupt function for execution every 1 millisecond */
        TIM_Start(BOARD_TIM_PORT);

        while(reset == CO_RESET_NOT){
/* loop for normal program execution ******************************************/
            uint16_t timer1msCopy, timer1msDiff;

            timer1msCopy = CO_timer1ms;
            timer1msDiff = timer1msCopy - timer1msPrevious;
            timer1msPrevious = timer1msCopy;

            /* CANopen process */
            reset = CO_process(CO, timer1msDiff, NULL);
        }
    }

/* program exit ***************************************************************/

    /* delete objects from memory */
    CO_delete(CO);

    /* reset */
    return 0;
}


/* timer thread executes in constant intervals ********************************/
void tmrTask_thread(void){

    INCREMENT_1MS(CO_timer1ms);

    if (CO->CANmodule[0]->CANnormal) {
        bool_t syncWas;

        /* Process Sync */
        syncWas = CO_process_SYNC(CO, TMR_TASK_INTERVAL);

        /* Read inputs */
        CO_process_RPDO(CO, syncWas);

        /* Further I/O or nonblocking application code may go here. */

        /* Write outputs */
        CO_process_TPDO(CO, syncWas, TMR_TASK_INTERVAL);

        /* verify timer overflow */
        if((TIM_GetInterruptStatus(BOARD_TIM_PORT) & TIM_STATUS_UPDATE_PERIOD) != 0u) {
            CO_errorReport(CO->em, CO_EM_ISR_TIMER_OVERFLOW, CO_EMC_SOFTWARE_INTERNAL, 0u);
            TIM_ClearInterruptStatus(BOARD_TIM_PORT, TIM_STATUS_UPDATE_PERIOD);
        }
    }
}

/* Setup the timer. */
void app_tim_init(void)
{
    /* Set the counter counting step. */
    TIM_Init_Type tim_init;
    tim_init.ClockFreqHz = BOARD_TIM_FREQ;
    tim_init.StepFreqHz = APP_TIM_UPDATE_STEP; /* 1ms. */
    tim_init.Period = APP_TIM_UPDATE_PERIOD - 1u;
    tim_init.EnablePreloadPeriod = false;
    tim_init.PeriodMode = TIM_PeriodMode_Continuous;
    tim_init.CountMode = TIM_CountMode_Increasing;
    TIM_Init(BOARD_TIM_PORT, &tim_init);

    /* Enable interrupt. */
    TIM_EnableInterrupts(BOARD_TIM_PORT, TIM_INT_UPDATE_PERIOD, true);
    NVIC_EnableIRQ(BOARD_TIM_IRQn);
}

/* CAN interrupt function *****************************************************/
void BOARD_FLEXCAN_IRQHandler(void)
{
    CO_CANinterrupt(CO->CANmodule[0]);
}

void BOARD_TIM_IRQHandler(void)
{
    TIM_ClearInterruptStatus(BOARD_TIM_PORT, TIM_STATUS_UPDATE_PERIOD);

    tmrTask_thread();
}
